#include "cartographer/mapping/internal/2d/pose_graph_2d.h"

#include <algorithm>
#include <cmath>
#include <cstdio>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <memory>
#include <sstream>
#include <string>

#include "Eigen/Eigenvalues"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/internal/voxel_filter.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        static auto *kWorkQueueDelayMetric = metrics::Gauge::Null();
        static auto *kWorkQueueSizeMetric = metrics::Gauge::Null();
        static auto *kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
        static auto *kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
        static auto *kActiveSubmapsMetric = metrics::Gauge::Null();
        static auto *kFrozenSubmapsMetric = metrics::Gauge::Null();
        static auto *kDeletedSubmapsMetric = metrics::Gauge::Null();

        /*
        节点类型(两种)：
            1）一帧scan与子图匹配后的位姿(trackingframe)，一个子图内有多个这种scan节点。
            2）子图原点的位姿，即子图节点。
        约束类型(两种)：
            1）scan节点与子图原点间的坐标变换，即子图内约束，就是子图内的scan相对于子图的坐标变换。
            2）闭环约束，
        */
       
        /*
        主逻辑：传入新的节点和各传感器数据后，均新增WorkItem，WorkItem通过回调函数定义一个约束计算任务;任务有依赖关系，在线程
            调度的时候，依赖任务先执行;约束就是点云位姿与子图位姿的相对变换关系;所有约束构建完成后会进行全局优化, 这个全局优化
            与前段ScanMatch一样也是调用ceres计算最小误差;回环检测就是暴力搜索，拿点云和所有子图进行匹配，就是计算点云在子图的
            栅格空间的概率总和，达到一定的阈值认为这个点云与子图存在回环;在回环的暴力搜索过程中使用分支定界法加速搜索;全局优化完
            后会把子图和和点云的全局位姿保存起来;
        概括：
            优化问题可以描述成一个图，图中节点是点云和子图在global系下的位姿，边是点云和子图的相对位姿(即约束)，
            这个约束可是子图内的，也可是回环检测约束。
        */
        PoseGraph2D::PoseGraph2D(
            const proto::PoseGraphOptions &options,
            std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
            common::ThreadPool *thread_pool)
            : options_(options),
              optimization_problem_(std::move(optimization_problem)),
              constraint_builder_(options_.constraint_builder_options(), thread_pool),
              thread_pool_(thread_pool)
        {
            if (options.has_overlapping_submaps_trimmer_2d())
            {
                const auto &trimmer_options = options.overlapping_submaps_trimmer_2d();
                AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(
                    trimmer_options.fresh_submaps_count(),
                    trimmer_options.min_covered_area(),
                    trimmer_options.min_added_submaps_count()));
            }
        }

        PoseGraph2D::~PoseGraph2D()
        {
            WaitForAllComputations();
            absl::MutexLock locker(&work_queue_mutex_);
            CHECK(work_queue_ == nullptr);
        }

        // 获取trajectory_id下的正处于活跃状态下的子图的SubmapId
        std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
            const int trajectory_id, const common::Time time,
            const std::vector<std::shared_ptr<const Submap2D>> &insertion_submaps)
        {
            CHECK(!insertion_submaps.empty());
            const auto &submap_data = optimization_problem_->submap_data();
            if (insertion_submaps.size() == 1)
            {
                // If we don't already have an entry for the first submap, add one.
                // 如果是刚开始的轨迹, 或者insertion_submaps.back()是第一次看到, 就添加新的子图
                if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
                {
                    if (data_.initial_trajectory_poses.count(trajectory_id) > 0)
                    {
                        data_.trajectory_connectivity_state.Connect(
                            trajectory_id,
                            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
                            time);
                    }
                    optimization_problem_->AddSubmap(
                        trajectory_id, transform::Project2D(
                                           ComputeLocalToGlobalTransform(
                                               data_.global_submap_poses_2d, trajectory_id)
                                           * insertion_submaps[0]->local_pose()));
                }
                CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
                const SubmapId submap_id{trajectory_id, 0};
                CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
                return {submap_id};
            }
            CHECK_EQ(2, insertion_submaps.size());
            const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
            CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
            const SubmapId last_submap_id = std::prev(end_it)->id;
            if (data_.submap_data.at(last_submap_id).submap == insertion_submaps.front())
            {
                // In this case, 'last_submap_id' is the ID of
                // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
                const auto &first_submap_pose = submap_data.at(last_submap_id).global_pose;
                optimization_problem_->AddSubmap(trajectory_id,
                                                 first_submap_pose * constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() * constraints::ComputeSubmapPose(*insertion_submaps[1]));
                return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
            }
            CHECK(data_.submap_data.at(last_submap_id).submap == insertion_submaps.back());
            const SubmapId front_submap_id{trajectory_id, last_submap_id.submap_index - 1};
            CHECK(data_.submap_data.at(front_submap_id).submap == insertion_submaps.front());
            return {front_submap_id, last_submap_id};
        }

        // 新增后端优化节点
        NodeId PoseGraph2D::AppendNode(
            std::shared_ptr<const TrajectoryNode::Data> constant_data,
            const int trajectory_id,
            const std::vector<std::shared_ptr<const Submap2D>> &insertion_submaps,
            const transform::Rigid3d &optimized_pose)
        {
            absl::MutexLock locker(&mutex_);
            // 如果轨迹不存在, 则将轨迹添加到连接状态里并添加采样器。这个轨迹并不是指前端部分的轨迹，在后端
            // 优化部分(PoseGraph)里有专门用来构建约束的"轨迹"，即由若干节点连接的抽线的轨迹。
            AddTrajectoryIfNeeded(trajectory_id);
            if (!CanAddWorkItemModifying(trajectory_id))
            {
                LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
            }
            // 加入当前轨迹中
            const NodeId node_id = data_.trajectory_nodes.Append(trajectory_id, TrajectoryNode{constant_data, optimized_pose});
            ++data_.num_trajectory_nodes;
            // Test if the 'insertion_submap.back()' is one we never saw before.
            // 如果是刚开始的轨迹, 或者insertion_submaps.back()是第一次看到, 就添加新的子图
            if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))->data.submap != insertion_submaps.back())
            {
                // 如果是新的submap,则需要加入到submap_data中
                // We grow 'data_.submap_data' as needed. This code assumes that the first
                // time we see a new submap is as 'insertion_submaps.back()'.
                const SubmapId submap_id = data_.submap_data.Append(trajectory_id, InternalSubmapData());
                data_.submap_data.at(submap_id).submap = insertion_submaps.back();
                LOG(INFO) << "Inserted submap " << submap_id << ".";
                kActiveSubmapsMetric->Increment();
            }
            return node_id;
        }

        /// @brief 在GlobalTrajectoryBuilder中被调用;前端匹配成功后返回匹配结果(包括点云数据，两个子图，local位姿等);
        /// GlobalTrajectoryBuilder将前端匹配结果直接给到该函数
        /// @param constant_data  匹配后的数据，包括时间,点云和local系的pose
        /// @param trajectory_id
        /// @param insertion_submaps
        /// @return
        NodeId PoseGraph2D::AddNode(
            std::shared_ptr<const TrajectoryNode::Data> constant_data,
            const int trajectory_id,
            const std::vector<std::shared_ptr<const Submap2D>> &insertion_submaps)
        {
            // 将节点在local坐标系下的坐标转成global坐标系下的坐标。前端匹配都是在local坐标系。
            const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
            // 加入节点, 并得到节点的id;这里的"节点"是后端优化模块的概念;
            const NodeId node_id = AppendNode(constant_data, trajectory_id, insertion_submaps, optimized_pose);
            // We have to check this here, because it might have changed by the time we execute the lambda.
            // 获取第一个submap是否是完成状态，如果完成即插入的帧数足够且不会再加入激光数据，则需要计算约束并进行全局优化。
            // 把计算约束的工作放入workitem中等待执行
            const bool newly_finished_submap = insertion_submaps.front()->insertion_finished();
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); });
            return node_id;
        }

        /*
         * 后端优化的操作基本都是采用线程池在后台运行，将任务放入任务队列中，然后排队调用执行
         */
        void PoseGraph2D::AddWorkItem(const std::function<WorkItem::Result()> &work_item)
        {
            absl::MutexLock locker(&work_queue_mutex_);
            if (work_queue_ == nullptr)
            {
                // 如果工作队列未被初始化，则先初始化并启动线程执行任务队列
                work_queue_ = absl::make_unique<WorkQueue>();
                auto task = absl::make_unique<common::Task>();
                task->SetWorkItem([this]() { DrainWorkQueue(); });
                thread_pool_->Schedule(std::move(task));
            }
            // 将当前任务插入到工作队列中，等待执行
            const auto now = std::chrono::steady_clock::now();
            work_queue_->push_back({now, work_item});
            kWorkQueueSizeMetric->Set(work_queue_->size());
            kWorkQueueDelayMetric->Set(
                std::chrono::duration_cast<std::chrono::duration<double>>(
                    now - work_queue_->front().time)
                    .count());
        }

        void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id)
        {
            data_.trajectories_state[trajectory_id];
            CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::FINISHED);
            CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::DELETED);
            CHECK(data_.trajectories_state.at(trajectory_id).deletion_state == InternalTrajectoryState::DeletionState::NORMAL);
            data_.trajectory_connectivity_state.Add(trajectory_id);
            // Make sure we have a sampler for this trajectory.
            if (!global_localization_samplers_[trajectory_id])
            {
                global_localization_samplers_[trajectory_id] = absl::make_unique<common::FixedRatioSampler>(
                    options_.global_sampling_ratio());
            }
        }

        void PoseGraph2D::AddImuData(const int trajectory_id, const sensor::ImuData &imu_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				if (CanAddWorkItemModifying(trajectory_id)) {
					optimization_problem_->AddImuData(trajectory_id, imu_data);
				}
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::AddOdometryData(const int trajectory_id, const sensor::OdometryData &odometry_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				if (CanAddWorkItemModifying(trajectory_id)) {
					optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
				}
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::AddFixedFramePoseData(const int trajectory_id,
                                                const sensor::FixedFramePoseData &fixed_frame_pose_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				if (CanAddWorkItemModifying(trajectory_id)) {
					optimization_problem_->AddFixedFramePoseData(trajectory_id,fixed_frame_pose_data);
				}
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::AddLandmarkData(int trajectory_id, const sensor::LandmarkData &landmark_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				if (CanAddWorkItemModifying(trajectory_id)) {
					for (const auto& observation : landmark_data.landmark_observations) {
						data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
							PoseGraphInterface::LandmarkNode::LandmarkObservation{
								trajectory_id, landmark_data.time,
								observation.landmark_to_tracking_transform,
								observation.translation_weight, observation.rotation_weight });
					}
				}
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        /// @brief 计算子图与节点约束, 即回环检测
        /// @param node_id
        /// @param submap_id
        void PoseGraph2D::ComputeConstraint(const NodeId &node_id, const SubmapId &submap_id)
        {
            bool maybe_add_local_constraint = false;
            bool maybe_add_global_constraint = false;
            const TrajectoryNode::Data *constant_data;
            const Submap2D *submap;
            {
                absl::MutexLock locker(&mutex_);
                CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
                if (!data_.submap_data.at(submap_id).submap->insertion_finished())
                {
                    // 如果是未完成状态的地图不进行约束计算
                    // Uplink server only receives grids when they are finished, so skip
                    // constraint search before that.
                    return;
                }
                // 获取该node和该submap中的node中较新的时间，即最后一次具有约束的时间
                const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
                // 两个轨迹的最后连接时间
                const common::Time last_connection_time = data_.trajectory_connectivity_state.LastConnectionTime(node_id.trajectory_id, submap_id.trajectory_id);
                // 如果节点和子图属于同一轨迹, 或者时间小于阈值,则只需进行局部搜索窗口的约束计算(对局部子图进行回环检测)
                // 最近有一个约束把该节点和子图的轨迹连接起来了,则有必要进行一个局部窗口搜索匹配.
                // 因为他们很可能能连接上.
                if (node_id.trajectory_id == submap_id.trajectory_id || node_time < last_connection_time + common::FromSeconds(options_.global_constraint_search_after_n_seconds()))
                {
                    // If the node and the submap belong to the same trajectory or if there
                    // has been a recent global constraint that ties that node's trajectory to
                    // the submap's trajectory, it suffices to do a match constrained to a
                    // local search window.
                    maybe_add_local_constraint = true;
                }
                // 如果两个不属于同一条轨迹 并且 最近也没有什么连接. 则说明局部窗口搜索很可能连接不上,因此需要进行全局搜索.
                //  如果节点与子图不属于同一条轨迹 并且 间隔了一段时间, 同时采样器为true才进行 全局搜索窗口 的约束计算(对
                //  整体子图进行回环检测)
                else if (global_localization_samplers_[node_id.trajectory_id]->Pulse())
                {
                    maybe_add_global_constraint = true;
                }
                // 获取节点信息数据与地图数据
                constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
                submap = static_cast<const Submap2D *>(data_.submap_data.at(submap_id).submap.get());
            }
            if (maybe_add_local_constraint)
            {
                // 建图模式时执行, node_id.trajectory_id与submap_id.trajectory_id是相同的，last_connection_time没有意义会被忽略

                // 计算约束的先验估计值，都变换到global系，然后计算两个坐标的相对变换关系
                // submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点与节点的坐标变换
                const transform::Rigid2d initial_relative_pose = optimization_problem_->submap_data()
                                                                     .at(submap_id)
                                                                     .global_pose.inverse()
                                                                 * optimization_problem_->node_data().at(node_id).global_pose_2d;
                // 计算回环约束，通过局部搜索进行回环检测(对局部子图进行回环检测)
                constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data, initial_relative_pose);
            }
            else if (maybe_add_global_constraint)
            {
                // 定位模式时执行, node_id.trajectory_id 与 submap_id.trajectory_id 是不一致的
                // 全局搜索窗口的约束计算 (对整体子图进行回环检测)
                constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id, constant_data);
            }
        }

        // 子图内约束计算
        WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(const NodeId &node_id,
                                                                std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
                                                                const bool newly_finished_submap)
        {
            std::vector<SubmapId> submap_ids;
            std::vector<SubmapId> finished_submap_ids;
            std::set<NodeId> newly_finished_submap_node_ids;
            {
                absl::MutexLock locker(&mutex_);
                // 得到该节点的数据.
                const auto &constant_data = data_.trajectory_nodes.at(node_id).constant_data;
                // 根据节点数据的时间获取最新的submap的id
                submap_ids = InitializeGlobalSubmapPoses(node_id.trajectory_id, constant_data->time, insertion_submaps);
                CHECK_EQ(submap_ids.size(), insertion_submaps.size());
                const SubmapId matching_id = submap_ids.front();
                // 当前节点在局部地图中的位姿
                const transform::Rigid2d local_pose_2d = transform::Project2D(constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
                // 当前节点在全局地图中的位姿
                const transform::Rigid2d global_pose_2d = optimization_problem_->submap_data().at(matching_id).global_pose * constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() * local_pose_2d;
                // 把当前节点加入到优化问题
                optimization_problem_->AddTrajectoryNode(
                    matching_id.trajectory_id,
                    optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                                             global_pose_2d,
                                             constant_data->gravity_alignment});
                // 加入当前节点和插入的子图之间的约束.插入的子图一般是两个
                for (size_t i = 0; i < insertion_submaps.size(); ++i)
                {
                    const SubmapId submap_id = submap_ids[i];
                    // Even if this was the last node added to 'submap_id', the submap will
                    // only be marked as finished in 'data_.submap_data' further below.
                    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kNoConstraintSearch);
                    data_.submap_data.at(submap_id).node_ids.emplace(node_id);
                    // 计算相对位姿，即scan节点与子图节点的约束
                    const transform::Rigid2d constraint_transform = constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * local_pose_2d;
                    data_.constraints.push_back(
                        Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), options_.matcher_translation_weight(), options_.matcher_rotation_weight()}, Constraint::INTRA_SUBMAP});
                }

                // TODO(gaschler): Consider not searching for constraints against
                // trajectories scheduled for deletion.
                // TODO(danielsievers): Add a member variable and avoid having to copy
                // them out here.
                // 得到所有完成了的submap
                for (const auto &submap_id_data : data_.submap_data)
                {
                    if (submap_id_data.data.state == SubmapState::kFinished)
                    {
                        CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
                        finished_submap_ids.emplace_back(submap_id_data.id);
                    }
                }
                // 如果最新的子图已经完成,把state设置为完成.
                if (newly_finished_submap)
                {
                    const SubmapId newly_finished_submap_id = submap_ids.front();
                    InternalSubmapData &finished_submap_data = data_.submap_data.at(newly_finished_submap_id);
                    CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);
                    finished_submap_data.state = SubmapState::kFinished;
                    newly_finished_submap_node_ids = finished_submap_data.node_ids;
                }
            }
            /*
            匹配的过程就是建立约束的过程，也就是回环检测的过程，因采用的是图优化，有节点和约束的概念，节点可以理解为激光点的
            位姿，约束就是激光数据与子图之间相对位姿。采用暴力搜索的方式，计算量巨大
            */
            // 新加入的节点与过去所有的子图(已完成)进行匹配。节点与子图进行回环检测
            for (const auto &submap_id : finished_submap_ids)
            {
                ComputeConstraint(node_id, submap_id);
            }
            // 新建立的子图，与过去所有的节点进行匹配。子图与节点进行回环检测。
            if (newly_finished_submap)
            {
                const SubmapId newly_finished_submap_id = submap_ids.front();
                // We have a new completed submap, so we look into adding constraints for old nodes.
                for (const auto &node_id_data : optimization_problem_->node_data())
                {
                    const NodeId &node_id = node_id_data.id;
                    if (newly_finished_submap_node_ids.count(node_id) == 0)
                    {
                        ComputeConstraint(node_id, newly_finished_submap_id);
                    }
                }
            }
            // 约束计算完毕.
            constraint_builder_.NotifyEndOfNode();
            absl::MutexLock locker(&mutex_);
            // 判断是否需要进行优化.
            ++num_nodes_since_last_loop_closure_;
            if (options_.optimize_every_n_nodes() > 0 && num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes())
            {
                return WorkItem::Result::kRunOptimization;
            }
            return WorkItem::Result::kDoNotRunOptimization;
        }

        common::Time PoseGraph2D::GetLatestNodeTime(const NodeId &node_id,
                                                    const SubmapId &submap_id) const
        {
            common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
            const InternalSubmapData &submap_data = data_.submap_data.at(submap_id);
            if (!submap_data.node_ids.empty())
            {
                const NodeId last_submap_node_id = *data_.submap_data.at(submap_id).node_ids.rbegin();
                time = std::max(
                    time,
                    data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
            }
            return time;
        }

        void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint &constraint)
        {
            CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
            const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id);
            data_.trajectory_connectivity_state.Connect(
                constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id,
                time);
        }

        void PoseGraph2D::DeleteTrajectoriesIfNeeded()
        {
            TrimmingHandle trimming_handle(this);
            for (auto &it : data_.trajectories_state)
            {
                if (it.second.deletion_state == InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION)
                {
                    // TODO(gaschler): Consider directly deleting from data_, which may be
                    // more complete.
                    auto submap_ids = trimming_handle.GetSubmapIds(it.first);
                    for (auto &submap_id : submap_ids)
                    {
                        trimming_handle.TrimSubmap(submap_id);
                    }
                    it.second.state = TrajectoryState::DELETED;
                    it.second.deletion_state = InternalTrajectoryState::DeletionState::NORMAL;
                }
            }
        }

        /// @brief 该函数与函数DrainWorkQueue连续地互相调用；DrainWorkQueue函数是不断得取出队列里的约束计算任务执行；
        /// HandleWorkQueue是在需要全局优化时被函数DrainWorkQueue调用；
        /// @param result
        void PoseGraph2D::HandleWorkQueue(const constraints::ConstraintBuilder2D::Result &result)
        {
            {
                absl::MutexLock locker(&mutex_);
                data_.constraints.insert(data_.constraints.end(), result.begin(), result.end());
            }
            RunOptimization(); // 进行全局优化
            if (global_slam_optimization_callback_)
            {
                std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
                std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
                {
                    absl::MutexLock locker(&mutex_);
                    const auto &submap_data = optimization_problem_->submap_data();
                    const auto &node_data = optimization_problem_->node_data();
                    for (const int trajectory_id : node_data.trajectory_ids())
                    {
                        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
                        {
                            continue;
                        }
                        trajectory_id_to_last_optimized_node_id.emplace(
                            trajectory_id,
                            std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
                        trajectory_id_to_last_optimized_submap_id.emplace(
                            trajectory_id,
                            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
                    }
                }
                global_slam_optimization_callback_(
                    trajectory_id_to_last_optimized_submap_id,
                    trajectory_id_to_last_optimized_node_id);
            }

            {
                absl::MutexLock locker(&mutex_);
                for (const Constraint &constraint : result)
                {
                    UpdateTrajectoryConnectivity(constraint);
                }
                DeleteTrajectoriesIfNeeded();
                TrimmingHandle trimming_handle(this);
                for (auto &trimmer : trimmers_)
                {
                    trimmer->Trim(&trimming_handle);
                }
                trimmers_.erase(
                    std::remove_if(trimmers_.begin(), trimmers_.end(), [](std::unique_ptr<PoseGraphTrimmer> &trimmer) { return trimmer->IsFinished(); }),
                    trimmers_.end());
                num_nodes_since_last_loop_closure_ = 0;

                // Update the gauges that count the current number of constraints.
                double inter_constraints_same_trajectory = 0;
                double inter_constraints_different_trajectory = 0;
                for (const auto &constraint : data_.constraints)
                {
                    if (constraint.tag == cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP)
                    {
                        continue;
                    }
                    if (constraint.node_id.trajectory_id == constraint.submap_id.trajectory_id)
                    {
                        ++inter_constraints_same_trajectory;
                    }
                    else
                    {
                        ++inter_constraints_different_trajectory;
                    }
                }
                kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
                kConstraintsDifferentTrajectoryMetric->Set(inter_constraints_different_trajectory);
            }
            DrainWorkQueue();
        }

        // 取出一个任务进行执行；如果该任务执行完后需要进行全局优化，则退出执行全局优化；否则继续取出下一个任务来执行
        void PoseGraph2D::DrainWorkQueue()
        {
            bool process_work_queue = true;
            size_t work_queue_size;
            // 任务的内容是计算约束
            while (process_work_queue)
            {
                std::function<WorkItem::Result()> work_item;
                {
                    absl::MutexLock locker(&work_queue_mutex_);
                    if (work_queue_->empty())
                    {
                        work_queue_.reset();
                        return;
                    }
                    work_item = work_queue_->front().task;
                    work_queue_->pop_front();
                    work_queue_size = work_queue_->size();
                    kWorkQueueSizeMetric->Set(work_queue_size);
                }
                // 如需要执行优化则退出循环取执行优化
                process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
            }
            LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
            // We have to optimize again.
            // constraint_builder里面的约束计算都结束后会调用这个回调函数；result里面包含constraint_builder找到的约束
            constraint_builder_.WhenDone(
                [this](const constraints::ConstraintBuilder2D::Result &result) {
                    HandleWorkQueue(result);
                });
        }

        void PoseGraph2D::WaitForAllComputations()
        {
            int num_trajectory_nodes;
            {
                absl::MutexLock locker(&mutex_);
                num_trajectory_nodes = data_.num_trajectory_nodes;
            }

            const int num_finished_nodes_at_start = constraint_builder_.GetNumFinishedNodes();

            auto report_progress = [this, num_trajectory_nodes,
                                    num_finished_nodes_at_start]() {
                // Log progress on nodes only when we are actually processing nodes.
                if (num_trajectory_nodes != num_finished_nodes_at_start)
                {
                    std::ostringstream progress_info;
                    progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
                                  << 100. * (constraint_builder_.GetNumFinishedNodes() - num_finished_nodes_at_start) / (num_trajectory_nodes - num_finished_nodes_at_start)
                                  << "%...";
                    std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
                }
            };

            // First wait for the work queue to drain so that it's safe to schedule
            // a WhenDone() callback.
            {
                const auto predicate = [this]()
                                           EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) {
                                               return work_queue_ == nullptr;
                                           };
                absl::MutexLock locker(&work_queue_mutex_);
                while (!work_queue_mutex_.AwaitWithTimeout(
                    absl::Condition(&predicate),
                    absl::FromChrono(common::FromSeconds(1.))))
                {
                    report_progress();
                }
            }

            // Now wait for any pending constraint computations to finish.
            absl::MutexLock locker(&mutex_);
            bool notification = false;
            constraint_builder_.WhenDone(
                [this,
                 &notification](const constraints::ConstraintBuilder2D::Result &result)
                    LOCKS_EXCLUDED(mutex_) {
                        absl::MutexLock locker(&mutex_);
                        data_.constraints.insert(data_.constraints.end(), result.begin(),
                                                 result.end());
                        notification = true;
                    });
            const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
                return notification;
            };
            while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
                                            absl::FromChrono(common::FromSeconds(1.))))
            {
                report_progress();
            }
            CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
            std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
        }

        void PoseGraph2D::DeleteTrajectory(const int trajectory_id)
        {
            {
                absl::MutexLock locker(&mutex_);
                auto it = data_.trajectories_state.find(trajectory_id);
                if (it == data_.trajectories_state.end())
                {
                    LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
                                 << trajectory_id;
                    return;
                }
                it->second.deletion_state = InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
            }
            AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				CHECK(data_.trajectories_state.at(trajectory_id).state !=
					TrajectoryState::ACTIVE);
				CHECK(data_.trajectories_state.at(trajectory_id).state !=
					TrajectoryState::DELETED);
				CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
					InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION);
				data_.trajectories_state.at(trajectory_id).deletion_state =
					InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::FinishTrajectory(const int trajectory_id)
        {
            AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				CHECK(!IsTrajectoryFinished(trajectory_id));
				data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;

				for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
					data_.submap_data.at(submap.id).state = SubmapState::kFinished;
				}
				return WorkItem::Result::kRunOptimization; });
        }

        bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const
        {
            return data_.trajectories_state.count(trajectory_id) != 0 && data_.trajectories_state.at(trajectory_id).state == TrajectoryState::FINISHED;
        }

        void PoseGraph2D::FreezeTrajectory(const int trajectory_id)
        {
            {
                absl::MutexLock locker(&mutex_);
                data_.trajectory_connectivity_state.Add(trajectory_id);
            }
            AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				CHECK(!IsTrajectoryFrozen(trajectory_id));
				// Connect multiple frozen trajectories among each other.
				// This is required for localization against multiple frozen trajectories
				// because we lose inter-trajectory constraints when freezing.
				for (const auto& entry : data_.trajectories_state) {
					const int other_trajectory_id = entry.first;
					if (!IsTrajectoryFrozen(other_trajectory_id)) {
						continue;
					}
					if (data_.trajectory_connectivity_state.TransitivelyConnected(
						trajectory_id, other_trajectory_id)) {
						// Already connected, nothing to do.
						continue;
					}
					data_.trajectory_connectivity_state.Connect(
						trajectory_id, other_trajectory_id, common::FromUniversal(0));
				}
				data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const
        {
            return data_.trajectories_state.count(trajectory_id) != 0 && data_.trajectories_state.at(trajectory_id).state == TrajectoryState::FROZEN;
        }

        void PoseGraph2D::AddSubmapFromProto(
            const transform::Rigid3d &global_submap_pose, const proto::Submap &submap)
        {
            if (!submap.has_submap_2d())
            {
                return;
            }

            const SubmapId submap_id = {submap.submap_id().trajectory_id(),
                                        submap.submap_id().submap_index()};

            const transform::Rigid2d global_submap_pose_2d = transform::Project2D(global_submap_pose);
            {
                absl::MutexLock locker(&mutex_);
                const std::shared_ptr<const Submap2D> submap_ptr = std::make_shared<const Submap2D>(submap.submap_2d(),
                                                                                                    &conversion_tables_);
                AddTrajectoryIfNeeded(submap_id.trajectory_id);
                if (!CanAddWorkItemModifying(submap_id.trajectory_id))
                    return;
                data_.submap_data.Insert(submap_id, InternalSubmapData());
                data_.submap_data.at(submap_id).submap = submap_ptr;
                // Immediately show the submap at the 'global_submap_pose'.
                data_.global_submap_poses_2d.Insert(
                    submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
            }

            // TODO(MichaelGrupp): MapBuilder does freezing before deserializing submaps,
            // so this should be fine.
            if (IsTrajectoryFrozen(submap_id.trajectory_id))
            {
                kFrozenSubmapsMetric->Increment();
            }
            else
            {
                kActiveSubmapsMetric->Increment();
            }

            AddWorkItem(
                [this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) {
                    absl::MutexLock locker(&mutex_);
                    data_.submap_data.at(submap_id).state = SubmapState::kFinished;
                    optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
                    return WorkItem::Result::kDoNotRunOptimization;
                });
        }

        void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d &global_pose,
                                           const proto::Node &node)
        {
            const NodeId node_id = {node.node_id().trajectory_id(),
                                    node.node_id().node_index()};
            std::shared_ptr<const TrajectoryNode::Data> constant_data = std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));

            {
                absl::MutexLock locker(&mutex_);
                AddTrajectoryIfNeeded(node_id.trajectory_id);
                if (!CanAddWorkItemModifying(node_id.trajectory_id))
                    return;
                data_.trajectory_nodes.Insert(node_id,
                                              TrajectoryNode{constant_data, global_pose});
            }

            AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				const auto& constant_data =
					data_.trajectory_nodes.at(node_id).constant_data;
				const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
					constant_data->gravity_alignment.inverse());
				optimization_problem_->InsertTrajectoryNode(
					node_id,
					optimization::NodeSpec2D{
						constant_data->time,
						transform::Project2D(constant_data->local_pose *
											 gravity_alignment_inverse),
						transform::Project2D(global_pose * gravity_alignment_inverse),
						constant_data->gravity_alignment });
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::SetTrajectoryDataFromProto(
            const proto::TrajectoryData &data)
        {
            TrajectoryData trajectory_data;
            // gravity_constant and imu_calibration are omitted as its not used in 2d

            if (data.has_fixed_frame_origin_in_map())
            {
                trajectory_data.fixed_frame_origin_in_map = transform::ToRigid3(data.fixed_frame_origin_in_map());

                const int trajectory_id = data.trajectory_id();
                AddWorkItem([this, trajectory_id, trajectory_data]()
                                LOCKS_EXCLUDED(mutex_) {
					absl::MutexLock locker(&mutex_);
					if (CanAddWorkItemModifying(trajectory_id)) {
						optimization_problem_->SetTrajectoryData(
							trajectory_id, trajectory_data);
					}
					return WorkItem::Result::kDoNotRunOptimization; });
            }
        }

        void PoseGraph2D::AddNodeToSubmap(const NodeId &node_id,
                                          const SubmapId &submap_id)
        {
            AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
					data_.submap_data.at(submap_id).node_ids.insert(node_id);
				}
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::AddSerializedConstraints(
            const std::vector<Constraint> &constraints)
        {
            AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				for (const auto& constraint : constraints) {
					CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
					CHECK(data_.submap_data.Contains(constraint.submap_id));
					CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data !=
						nullptr);
					CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr);
					switch (constraint.tag) {
					case Constraint::Tag::INTRA_SUBMAP:
						CHECK(data_.submap_data.at(constraint.submap_id)
							.node_ids.emplace(constraint.node_id)
							.second);
						break;
					case Constraint::Tag::INTER_SUBMAP:
						UpdateTrajectoryConnectivity(constraint);
						break;
					}
					const Constraint::Pose pose = {
						constraint.pose.zbar_ij *
							transform::Rigid3d::Rotation(
								data_.trajectory_nodes.at(constraint.node_id)
									.constant_data->gravity_alignment.inverse()),
						constraint.pose.translation_weight, constraint.pose.rotation_weight };
					data_.constraints.push_back(Constraint{
						constraint.submap_id, constraint.node_id, pose, constraint.tag });
				}
				LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer)
        {
            // C++11 does not allow us to move a unique_ptr into a lambda.
            PoseGraphTrimmer *const trimmer_ptr = trimmer.release();
            AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				trimmers_.emplace_back(trimmer_ptr);
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        void PoseGraph2D::RunFinalOptimization()
        {
            {
                AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
					absl::MutexLock locker(&mutex_);
					optimization_problem_->SetMaxNumIterations(
						options_.max_num_final_iterations());
					return WorkItem::Result::kRunOptimization; });
                AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
					absl::MutexLock locker(&mutex_);
					optimization_problem_->SetMaxNumIterations(
						options_.optimization_problem_options()
						.ceres_solver_options()
						.max_num_iterations());
					return WorkItem::Result::kDoNotRunOptimization; });
            }
            WaitForAllComputations();
        }

        /// @brief 执行优化, 并使用优化结果对保存的数据进行更新
        void PoseGraph2D::RunOptimization()
        {
            if (optimization_problem_->submap_data().empty())
            {
                return;
            }
            // No other thread is accessing the optimization_problem_,
            // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
            // when executing the Solve. Solve is time consuming, so not taking the mutex
            // before Solve to avoid blocking foreground processing.

            // Solve 比较耗时, 所以在执行 Solve 之前不要加互斥锁, 以免阻塞其他的任务处理; landmark直接参与优化问题
            // 这里是后端优化真正的入口
            optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes);
            absl::MutexLock locker(&mutex_);
            // 获取优化后的结果
            // submap_data的类型是 MapById<SubmapId, optimization::SubmapSpec2D>
            const auto &submap_data = optimization_problem_->submap_data();
            // node_data的类型是 MapById<NodeId, NodeSpec2D>
            // node_data是优化后的所有节点的新位姿
            const auto &node_data = optimization_problem_->node_data();
            // 更新轨迹内的节点位置
            for (const int trajectory_id : node_data.trajectory_ids())
            {
                for (const auto &node : node_data.trajectory(trajectory_id))
                {
                    auto &mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
                    // 进行对优化后的global位姿进行重力矫正，然后赋值给 data_.trajectory_nodes，
                    mutable_trajectory_node.global_pose = transform::Embed3D(node.data.global_pose_2d) * transform::Rigid3d::Rotation(mutable_trajectory_node.constant_data->gravity_alignment);
                }
                // Extrapolate all point cloud poses that were not included in the
                // 'optimization_problem_' yet.
                const auto local_to_new_global = ComputeLocalToGlobalTransform(submap_data, trajectory_id);
                const auto local_to_old_global = ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id);
                const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse();

                const NodeId last_optimized_node_id = std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
                auto node_it = std::next(data_.trajectory_nodes.find(last_optimized_node_id));
                for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id); ++node_it)
                {
                    auto &mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
                    mutable_trajectory_node.global_pose = old_global_to_new_global * mutable_trajectory_node.global_pose;
                }
            }
            for (const auto &landmark : optimization_problem_->landmark_data())
            {
                data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
            }
            data_.global_submap_poses_2d = submap_data;
        }

        bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id)
        {
            auto it = data_.trajectories_state.find(trajectory_id);
            if (it == data_.trajectories_state.end())
            {
                return true;
            }
            if (it->second.state == TrajectoryState::FINISHED)
            {
                // TODO(gaschler): Replace all FATAL to WARNING after some testing.
                LOG(FATAL) << "trajectory_id " << trajectory_id
                           << " has finished "
                              "but modification is requested, skipping.";
                return false;
            }
            if (it->second.deletion_state != InternalTrajectoryState::DeletionState::NORMAL)
            {
                LOG(FATAL) << "trajectory_id " << trajectory_id
                           << " has been scheduled for deletion "
                              "but modification is requested, skipping.";
                return false;
            }
            if (it->second.state == TrajectoryState::DELETED)
            {
                LOG(FATAL) << "trajectory_id " << trajectory_id
                           << " has been deleted "
                              "but modification is requested, skipping.";
                return false;
            }
            return true;
        }

        MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const
        {
            absl::MutexLock locker(&mutex_);
            return data_.trajectory_nodes;
        }

        MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
            const
        {
            MapById<NodeId, TrajectoryNodePose> node_poses;
            absl::MutexLock locker(&mutex_);
            for (const auto &node_id_data : data_.trajectory_nodes)
            {
                absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
                if (node_id_data.data.constant_data != nullptr)
                {
                    constant_pose_data = TrajectoryNodePose::ConstantPoseData{
                        node_id_data.data.constant_data->time,
                        node_id_data.data.constant_data->local_pose};
                }
                node_poses.Insert(
                    node_id_data.id,
                    TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data});
            }
            return node_poses;
        }

        std::map<int, PoseGraphInterface::TrajectoryState>
        PoseGraph2D::GetTrajectoryStates() const
        {
            std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
            absl::MutexLock locker(&mutex_);
            for (const auto &it : data_.trajectories_state)
            {
                trajectories_state[it.first] = it.second.state;
            }
            return trajectories_state;
        }

        std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
            const
        {
            std::map<std::string, transform::Rigid3d> landmark_poses;
            absl::MutexLock locker(&mutex_);
            for (const auto &landmark : data_.landmark_nodes)
            {
                // Landmark without value has not been optimized yet.
                if (!landmark.second.global_landmark_pose.has_value())
                    continue;
                landmark_poses[landmark.first] = landmark.second.global_landmark_pose.value();
            }
            return landmark_poses;
        }

        void PoseGraph2D::SetLandmarkPose(const std::string &landmark_id,
                                          const transform::Rigid3d &global_pose,
                                          const bool frozen)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
				absl::MutexLock locker(&mutex_);
				data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
				data_.landmark_nodes[landmark_id].frozen = frozen;
				return WorkItem::Result::kDoNotRunOptimization; });
        }

        sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->imu_data();
        }

        sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->odometry_data();
        }

        std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
        PoseGraph2D::GetLandmarkNodes() const
        {
            absl::MutexLock locker(&mutex_);
            return data_.landmark_nodes;
        }

        std::map<int, PoseGraphInterface::TrajectoryData>
        PoseGraph2D::GetTrajectoryData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->trajectory_data();
        }

        sensor::MapByTime<sensor::FixedFramePoseData>
        PoseGraph2D::GetFixedFramePoseData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->fixed_frame_pose_data();
        }

        std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const
        {
            std::vector<PoseGraphInterface::Constraint> result;
            absl::MutexLock locker(&mutex_);
            for (const Constraint &constraint : data_.constraints)
            {
                result.push_back(Constraint{
                    constraint.submap_id, constraint.node_id,
                    Constraint::Pose{constraint.pose.zbar_ij * transform::Rigid3d::Rotation(data_.trajectory_nodes.at(constraint.node_id).constant_data->gravity_alignment),
                                     constraint.pose.translation_weight,
                                     constraint.pose.rotation_weight},
                    constraint.tag});
            }
            return result;
        }

        void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
                                                   const int to_trajectory_id,
                                                   const transform::Rigid3d &pose,
                                                   const common::Time time)
        {
            absl::MutexLock locker(&mutex_);
            data_.initial_trajectory_poses[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time};
        }

        transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
            const int trajectory_id, const common::Time time) const
        {
            CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
            const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);
            if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id))
            {
                return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)->data.global_pose;
            }
            if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
            {
                return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
                    ->data.global_pose;
            }
            return transform::Interpolate(
                       transform::TimestampedTransform{std::prev(it)->data.time(),
                                                       std::prev(it)->data.global_pose},
                       transform::TimestampedTransform{it->data.time(),
                                                       it->data.global_pose},
                       time)
                .transform;
        }

        // 将节点在local坐标系下的位姿转成global坐标系下的位姿
        transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(const int trajectory_id) const
        {
            // 可能同时间有多个线程调用这同一个函数, 所以要加锁
            absl::MutexLock locker(&mutex_);
            return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id);
        }

        std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const
        {
            absl::MutexLock locker(&mutex_);
            return data_.trajectory_connectivity_state.Components();
        }

        PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
            const SubmapId &submap_id) const
        {
            absl::MutexLock locker(&mutex_);
            return GetSubmapDataUnderLock(submap_id);
        }

        MapById<SubmapId, PoseGraphInterface::SubmapData>
        PoseGraph2D::GetAllSubmapData() const
        {
            absl::MutexLock locker(&mutex_);
            return GetSubmapDataUnderLock();
        }

        MapById<SubmapId, PoseGraphInterface::SubmapPose>
        PoseGraph2D::GetAllSubmapPoses() const
        {
            absl::MutexLock locker(&mutex_);
            MapById<SubmapId, SubmapPose> submap_poses;
            for (const auto &submap_id_data : data_.submap_data)
            {
                auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
                submap_poses.Insert(
                    submap_id_data.id,
                    PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
                                          submap_data.pose});
            }
            return submap_poses;
        }

        /// @brief 返回local系到全局系的转换关系;全局系一般是map系，如果配置文件要求提供odom系，那么local就是odom系，对于一个轨迹
        /// 来讲，局部坐标系就是轨迹开始时机器人的位姿，所以开始的时候odom与map重合，后面出现累计误差则odom出现漂移；odom的漂移量就是
        /// 该函数的返回值；具体的求法就是倒数第二个子图的优化后的位姿乘上它的局部位姿的逆，因为局部位姿是没有经过优化的；可以理解为odom的
        /// 漂移量就是估计的位姿和优化后的位姿之差；
        /// @param global_submap_poses
        /// @param trajectory_id
        /// @return
        transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
            const MapById<SubmapId, optimization::SubmapSpec2D> &global_submap_poses, const int trajectory_id) const
        {
            auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
            auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
            if (begin_it == end_it)
            {
                // 该轨迹还没有任何已经优化后的子图
                const auto it = data_.initial_trajectory_poses.find(trajectory_id);
                if (it != data_.initial_trajectory_poses.end())
                {
                    // 该轨迹设置了初始的位姿
                    return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, it->second.time)
                           * it->second.relative_pose;
                }
                else
                {
                    // 该轨迹没有设置初始位姿，所以初始的时候odom与map是重合的
                    return transform::Rigid3d::Identity();
                }
            }
            // 倒数第二个已优化后的子图的ID
            const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
            // Accessing 'local_pose' in Submap is okay, since the member is const.
            // 该子图优化后的位姿
            const auto &optimized_pose = transform::Embed3D(global_submap_poses.at(last_optimized_submap_id).global_pose);
            // 该子图的局部位姿，即没有经过优化的位姿
            const auto &local_pose = data_.submap_data.at(last_optimized_submap_id).submap->local_pose();
            return optimized_pose * local_pose.inverse();
        }

        PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(const SubmapId &submap_id) const
        {
            const auto it = data_.submap_data.find(submap_id);
            if (it == data_.submap_data.end())
            {
                return {};
            }
            auto submap = it->data.submap;
            // 如果该子图已经优化过，则其位姿会在global_submap_poses_2d中保存
            if (data_.global_submap_poses_2d.Contains(submap_id))
            {
                // We already have an optimized pose.
                return {submap, transform::Embed3D(data_.global_submap_poses_2d.at(submap_id).global_pose)};
            }
            // 该子图没有优化，则找其所在轨迹中最后一个已优化的子图，用这个子图的全局位姿乘以该子图的相对位姿
            // We have to extrapolate.
            return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, submap_id.trajectory_id)
                                * submap->local_pose()};
        }

        PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D *const parent)
            : parent_(parent)
        {
        }

        int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const
        {
            const auto &submap_data = parent_->optimization_problem_->submap_data();
            return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
        }

        MapById<SubmapId, PoseGraphInterface::SubmapData>
        PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const
        {
            MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
            for (const auto &submap_id_data : parent_->data_.submap_data)
            {
                if (submap_id_data.data.state != SubmapState::kFinished || !parent_->data_.global_submap_poses_2d.Contains(submap_id_data.id))
                {
                    continue;
                }
                submaps.Insert(
                    submap_id_data.id,
                    SubmapData{submap_id_data.data.submap,
                               transform::Embed3D(parent_->data_.global_submap_poses_2d
                                                      .at(submap_id_data.id)
                                                      .global_pose)});
            }
            return submaps;
        }

        std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
            int trajectory_id) const
        {
            std::vector<SubmapId> submap_ids;
            const auto &submap_data = parent_->optimization_problem_->submap_data();
            for (const auto &it : submap_data.trajectory(trajectory_id))
            {
                submap_ids.push_back(it.id);
            }
            return submap_ids;
        }

        const MapById<NodeId, TrajectoryNode> &
        PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const
        {
            return parent_->data_.trajectory_nodes;
        }

        const std::vector<PoseGraphInterface::Constraint> &
        PoseGraph2D::TrimmingHandle::GetConstraints() const
        {
            return parent_->data_.constraints;
        }

        bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const
        {
            return parent_->IsTrajectoryFinished(trajectory_id);
        }

        void PoseGraph2D::TrimmingHandle::SetTrajectoryState(int trajectory_id,
                                                             TrajectoryState state)
        {
            parent_->data_.trajectories_state[trajectory_id].state = state;
        }

        void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId &submap_id)
        {
            // TODO(hrapp): We have to make sure that the trajectory has been finished
            // if we want to delete the last submaps.
            CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished);

            // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
            // once the submap with 'submap_id' is gone.
            // We need to use node_ids instead of constraints here to be also compatible
            // with frozen trajectories that don't have intra-constraints.
            std::set<NodeId> nodes_to_retain;
            for (const auto &submap_data : parent_->data_.submap_data)
            {
                if (submap_data.id != submap_id)
                {
                    nodes_to_retain.insert(submap_data.data.node_ids.begin(),
                                           submap_data.data.node_ids.end());
                }
            }

            // Remove all nodes that are exlusively associated to 'submap_id'.
            std::set<NodeId> nodes_to_remove;
            std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
                                parent_->data_.submap_data.at(submap_id).node_ids.end(),
                                nodes_to_retain.begin(), nodes_to_retain.end(),
                                std::inserter(nodes_to_remove, nodes_to_remove.begin()));

            // Remove all 'data_.constraints' related to 'submap_id'.
            {
                std::vector<Constraint> constraints;
                for (const Constraint &constraint : parent_->data_.constraints)
                {
                    if (constraint.submap_id != submap_id)
                    {
                        constraints.push_back(constraint);
                    }
                }
                parent_->data_.constraints = std::move(constraints);
            }

            // Remove all 'data_.constraints' related to 'nodes_to_remove'.
            // If the removal lets other submaps lose all their inter-submap constraints,
            // delete their corresponding constraint submap matchers to save memory.
            {
                std::vector<Constraint> constraints;
                std::set<SubmapId> other_submap_ids_losing_constraints;
                for (const Constraint &constraint : parent_->data_.constraints)
                {
                    if (nodes_to_remove.count(constraint.node_id) == 0)
                    {
                        constraints.push_back(constraint);
                    }
                    else
                    {
                        // A constraint to another submap will be removed, mark it as affected.
                        other_submap_ids_losing_constraints.insert(constraint.submap_id);
                    }
                }
                parent_->data_.constraints = std::move(constraints);
                // Go through the remaining constraints to ensure we only delete scan
                // matchers of other submaps that have no inter-submap constraints left.
                for (const Constraint &constraint : parent_->data_.constraints)
                {
                    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP)
                    {
                        continue;
                    }
                    else if (other_submap_ids_losing_constraints.count(
                                 constraint.submap_id))
                    {
                        // This submap still has inter-submap constraints - ignore it.
                        other_submap_ids_losing_constraints.erase(constraint.submap_id);
                    }
                }
                // Delete scan matchers of the submaps that lost all constraints.
                // TODO(wohe): An improvement to this implementation would be to add the
                // caching logic at the constraint builder which could keep around only
                // recently used scan matchers.
                for (const SubmapId &submap_id : other_submap_ids_losing_constraints)
                {
                    parent_->constraint_builder_.DeleteScanMatcher(submap_id);
                }
            }

            // Mark the submap with 'submap_id' as trimmed and remove its data.
            CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished);
            parent_->data_.submap_data.Trim(submap_id);
            parent_->constraint_builder_.DeleteScanMatcher(submap_id);
            parent_->optimization_problem_->TrimSubmap(submap_id);

            // We have one submap less, update the gauge metrics.
            kDeletedSubmapsMetric->Increment();
            if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id))
            {
                kFrozenSubmapsMetric->Decrement();
            }
            else
            {
                kActiveSubmapsMetric->Decrement();
            }

            // Remove the 'nodes_to_remove' from the pose graph and the optimization
            // problem.
            for (const NodeId &node_id : nodes_to_remove)
            {
                parent_->data_.trajectory_nodes.Trim(node_id);
                parent_->optimization_problem_->TrimTrajectoryNode(node_id);
            }
        }

        MapById<SubmapId, PoseGraphInterface::SubmapData>
        PoseGraph2D::GetSubmapDataUnderLock() const
        {
            MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
            for (const auto &submap_id_data : data_.submap_data)
            {
                submaps.Insert(submap_id_data.id,
                               GetSubmapDataUnderLock(submap_id_data.id));
            }
            return submaps;
        }

        void PoseGraph2D::SetGlobalSlamOptimizationCallback(
            PoseGraphInterface::GlobalSlamOptimizationCallback callback)
        {
            global_slam_optimization_callback_ = callback;
        }

        void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory *family_factory)
        {
            auto *latency = family_factory->NewGaugeFamily(
                "mapping_2d_pose_graph_work_queue_delay",
                "Age of the oldest entry in the work queue in seconds");
            kWorkQueueDelayMetric = latency->Add({});
            auto *queue_size = family_factory->NewGaugeFamily("mapping_2d_pose_graph_work_queue_size",
                                                              "Number of items in the work queue");
            kWorkQueueSizeMetric = queue_size->Add({});
            auto *constraints = family_factory->NewGaugeFamily(
                "mapping_2d_pose_graph_constraints",
                "Current number of constraints in the pose graph");
            kConstraintsDifferentTrajectoryMetric = constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
            kConstraintsSameTrajectoryMetric = constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}});
            auto *submaps = family_factory->NewGaugeFamily(
                "mapping_2d_pose_graph_submaps", "Number of submaps in the pose graph.");
            kActiveSubmapsMetric = submaps->Add({{"state", "active"}});
            kFrozenSubmapsMetric = submaps->Add({{"state", "frozen"}});
            kDeletedSubmapsMetric = submaps->Add({{"state", "deleted"}});
        }

    } // namespace mapping
} // namespace cartographer
